#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <thread>
#include <vector>
#include "./sdk/include/celex5/celex5.h"
#include "./sdk/include/celextypes.h"
#include "std_msgs/Float64MultiArray.h"
#include "std_msgs/UInt32MultiArray.h"
#include "event_recorder/CameraData.h"
#include "common_constants.h"

using namespace std;

/*
获取当前时间
@return:
    double 当前的时间戳（单位为秒）
*/
double time() {
    ros::Time rosTime = ros::Time::now();
    double res = rosTime.toSec();
    return res;
}

inline int ceil(int a, int b) {
    return a / b + (a % b ? 1 : 0);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "event_api");
    ros::NodeHandle n;
    ros::Publisher nodePub =
        n.advertise<event_recorder::CameraData>("/event_recorder/sub_event_res", 1);

    CeleX5* pCeleX = new CeleX5;
    if (pCeleX == NULL) {
        printf("\033[31mFailed to initialize event camera.\033[0m\n");
    } else {
        printf("\033[32mSuccessfully initialized event camera.\033[0m\n");
    }

    time_t lastCameraStamp = 0.0;
    double maxInnerStamp = 0.0;
    double eventStart = 0.0;
    double eventEnd = 0.0;
    double imuStart = 0.0;
    double imuEnd = 0.0;
    double eventLast = 0.0;
    double imuLast = 0.0;
    double currentInnerBase = 0.0;

    // vector<vector<unsigned int> > savedEventsInRow;
    // for (int i = 0; i < HEIGHT; i++) {
    //     vector<unsigned int> e;
    //     savedEventsInRow.push_back(e);
    // }

    string configPath = getenv("HOME");
    configPath += "/record_ws/src/event_recorder/config/";
    pCeleX->setSensorCfgFileDir(configPath);
    pCeleX->setFpnFileDir(configPath);
    pCeleX->setThreshold(INTENSITY_THRESHOLD);
    pCeleX->disableFrameModule();
    pCeleX->enableEventDenoising();
    pCeleX->openSensor(CeleX5::CeleX5_MIPI);
    pCeleX->setSensorFixedMode(CeleX5::Event_Intensity_Mode);

    ros::Rate frequency(USEC / DURATION_USEC);

    while (ros::ok()) {
        double msgTime = time();

        vector<EventData> rawRes;
        vector<IMUData> imuRawRes;
        unsigned int resLen;
        time_t cameraStamp = lastCameraStamp;

        if (pCeleX->isSensorReady()) {
            pCeleX->getEventDataVector(rawRes, resLen, cameraStamp);
            pCeleX->getIMUData(imuRawRes);
        }

        vector<EventData> res(rawRes);
        vector<IMUData> imuRes(imuRawRes);
        if (!rawRes.empty())
            rawRes.clear();
        if (!imuRawRes.empty())
            imuRawRes.clear();

        // vector<vector<unsigned int> > eventsInRow;
        // for (int i = 0; i < HEIGHT; i++) {
        //     vector<unsigned int> e;
        //     eventsInRow.push_back(e);
        // }
        
        event_recorder::CameraData pubRes;

        if (!res.empty() and (cameraStamp != lastCameraStamp)) {
            int sampleRatio = max((int)(res.size() / FINAL_EVENTS), 1);
            lastCameraStamp = cameraStamp;
            bool eventTimeRecorded = false;

            for (int i = 0; i < res.size(); i++) {
                double currentInnerStamp = res.at(i).tOffPixelIncreasing * 1e-6;
                maxInnerStamp = currentInnerStamp > maxInnerStamp
                                    ? currentInnerStamp
                                    : maxInnerStamp;
                if (currentInnerBase == 0.0) {
                    currentInnerBase = msgTime - currentInnerStamp;
                }
                if (eventLast > currentInnerStamp &&
                    eventLast > maxInnerStamp - 0.1 &&
                    currentInnerStamp < 0.1 &&
                    eventLast - currentInnerStamp >= 5.0) {
                    currentInnerBase += maxInnerStamp;
                }
                eventLast = currentInnerStamp;
                double currentTime = currentInnerBase + currentInnerStamp;
                if (!eventTimeRecorded) {
                    eventStart = currentTime;
                    eventTimeRecorded = true;
                }
                eventStart =
                    currentTime < eventStart ? currentTime : eventStart;
                eventEnd = currentTime > eventEnd ? currentTime : eventEnd;

                if (!(i % sampleRatio)) {
                    unsigned int currentSec = (unsigned int)currentTime;
                    unsigned int currentNSec =
                        (unsigned int)((currentTime - currentSec) * 1000000000);
                    unsigned int currentP = res.at(i).polarity > 0;
                    unsigned int currentY = HEIGHT - 1 - res.at(i).row;
                    unsigned int currentX = res.at(i).col;
                    pubRes.events.push_back(currentSec);
                    pubRes.events.push_back(currentNSec);
                    pubRes.events.push_back(currentP);
                    pubRes.events.push_back(currentY);
                    pubRes.events.push_back(currentX);
                    // eventsInRow[res.at(i).row].push_back(currentSec);
                    // eventsInRow[res.at(i).row].push_back(currentNSec);
                    // eventsInRow[res.at(i).row].push_back(currentP);
                    // eventsInRow[res.at(i).row].push_back(currentY);
                    // eventsInRow[res.at(i).row].push_back(currentX);
                }
            }
            res.clear();
        }

        // for (int i = 0; i < HEIGHT; i++) {
        //     if (!eventsInRow[i].size()) {
        //         for (int j = 0; j < savedEventsInRow[i].size(); j++) {
        //             pubRes.events.push_back(savedEventsInRow[i].at(j));
        //         }
        //     }
        //     savedEventsInRow[i] = eventsInRow[i];
        // }

        if (!imuRes.empty()) {
            bool imuTimeRecorded = false;

            for (int i = 0; i < imuRes.size(); i++) {
                double imuTimeStamp = imuRes.at(i).timestamp * 1e-3;
                double imuOmegaZ = imuRes.at(i).zGYROS;
                double imuOmegaY = imuRes.at(i).yGYROS;
                double imuOmegaX = imuRes.at(i).xGYROS;
                if (!imuTimeRecorded) {
                    imuStart = imuTimeStamp;
                    imuTimeRecorded = true;
                }
                imuStart = imuTimeStamp < imuStart ? imuTimeStamp : imuStart;
                imuEnd = imuTimeStamp > imuEnd ? imuTimeStamp : imuEnd;
                pubRes.imus.push_back(imuTimeStamp);
                pubRes.imus.push_back(imuOmegaZ);
                pubRes.imus.push_back(imuOmegaY);
                pubRes.imus.push_back(imuOmegaX);
                imuLast = imuTimeStamp;
            }
            imuRes.clear();
        }


        if (!pubRes.events.empty()) {
            int finalEventSize = pubRes.events.size();
            int finalIMUSize = pubRes.imus.size();
            pubRes.header.stamp.sec = (int)msgTime;
            pubRes.header.stamp.nsec =
                (int)((msgTime - pubRes.header.stamp.sec) * NSEC);
            pubRes.eventcol.push_back("t_sec");
            pubRes.eventcol.push_back("t_nsec");
            pubRes.eventcol.push_back("p");
            pubRes.eventcol.push_back("y");
            pubRes.eventcol.push_back("x");
            pubRes.imucol.push_back("t");
            pubRes.imucol.push_back("angvel_z");
            pubRes.imucol.push_back("angvel_y");
            pubRes.imucol.push_back("angvel_x");
            nodePub.publish(pubRes);
        }

        if (pCeleX->isSensorReady()) {
            // string startColor = finalEventSize ? "\033[36m" : "\033[35m";
            // string formatStr = "Time %.6f, %d events, %d IMUs.\n";
            // string endColor = finalEventSize ? "\033[0m" : "\033[0m";
            // printf((startColor + formatStr + endColor).data(), msgTime,
            // finalEventSize,
            //     finalIMUSize);
        } else {
            printf("\033[31mThe event camera is disconnected.\033[0m\n");
        }

        frequency.sleep();
    }
    return 0;
}